package edu.asu.sapa.heuristic;

import ilog.concert.IloConversion;
import ilog.concert.IloException;
import ilog.concert.IloLinearNumExpr;
import ilog.concert.IloNumVar;
import ilog.concert.IloNumVarType;
import ilog.concert.IloRange;
import ilog.cplex.IloCplex;
import ilog.cplex.IloCplex.UnknownObjectException;

import java.util.ArrayList;
import java.util.Arrays;
import java.util.HashMap;
import java.util.LinkedHashSet;
import java.util.LinkedList;
import java.util.List;
import java.util.Random;
import java.util.Set;
import java.util.Map.Entry;

import org.jgrapht.DirectedGraph;

import edu.asu.sapa.basic_ds.GAction;
import edu.asu.sapa.basic_ds.GoalManager;
import edu.asu.sapa.complex_ds.StateInfo;
import edu.asu.sapa.sas.Atom;
import edu.asu.sapa.sas.Effect;
import edu.asu.sapa.sas.EffectArc;
import edu.asu.sapa.sas.Operator;
import edu.asu.sapa.sas.PSPInterface;
import edu.asu.sapa.sas.Prevail;
import edu.asu.sapa.sas.SAS;
import edu.asu.sapa.sas.ValueVertex;
import edu.asu.sapa.sas.Variable;

public class SASOptRelax implements GoalHeuristic<PSPInterface> {
	static final double EPSILON = 1E-6;
	// static final double EPSILON = 0.1;

	protected IloCplex cplex;

	/** action variables */
	protected IloNumVar[][] X;

	protected IloNumVar[][] Z;

	protected IloRange[][] consFlow;

	protected IloRange[][] consCompFlow;

	protected SAS sas;

	protected ArrayList<DirectedGraph<ValueVertex, EffectArc>> dtg;

	protected float limit = Float.MAX_VALUE;

	protected boolean solveRelaxation;

	protected boolean solved;

	protected HashMap<IloNumVar, GoalVarVal> gMap = new HashMap<IloNumVar, GoalVarVal>();

	protected PSPInterface psp;

	private IloNumVar[] Gd;

	public boolean noActions;

	public double hValue;

	private boolean pddl3 = false;

	private HashMap<String, Boolean> compositionMap = new HashMap<String, Boolean>();

	public SASOptRelax(SAS sas, boolean pddl3) {
		this.sas = sas;
		this.pddl3 = pddl3;
	}

	// Dynamic model building!!!
	// 1 Create base model (all constraints and variables, *not* including
	// composition constraints)
	// 2 Solve base model
	// 3 while(action in solution i.e. value > EPSILON) {
	// if(action.getActive() == 0) {
	// action.setActive(1);
	// createCompositionConstraint(action) // where action is child operator
	// }
	// 4 Solve new model
	// }
	public void buildDynamicModel() throws EncodingBuildException {
		System.out.println("BUILDING DYNAMIC MODEL");
		try {
			solved = false;
			cplex = new IloCplex();

			X = new IloNumVar[sas.getOperators().size()][];
			for (Operator a : sas.getOperators()) {
				X[a.getID()] = new IloNumVar[a.getChildren().size()];
			}
			Z = new IloNumVar[sas.getVariables().size()][];
			for (Variable v : sas.getVariables()) {
				Z[v.getID()] = new IloNumVar[v.getDomain()];
			}
			Gd = new IloNumVar[psp.getGM().numGd];

			consFlow = new IloRange[sas.getVariables().size()][];
			for (Variable v : sas.getVariables()) {
				consFlow[v.getID()] = new IloRange[v.getDomain()];
			}

			dtg = sas.createDTG();
			createActionVariables();
			createGoalVariables();

			// Create base model
			if (!pddl3)
				createObjective();
			else
				createPDDL3Objective();

			createGoalConstraints();
			createFlowConstraints();
			createGoalDep1Constraints();
			createGoalDep2Constraints();
			createConversions();

			// Solve base model
			solveRelaxation = false;
			cplex.setParam(IloCplex.IntParam.MIPDisplay, 0);
			cplex.setParam(IloCplex.IntParam.BarDisplay, 0);
			cplex.setParam(IloCplex.IntParam.ConflictDisplay, 0);
			cplex.setParam(IloCplex.IntParam.NetDisplay, 0);
			cplex.setParam(IloCplex.IntParam.SimDisplay, 0); // ??
			cplex.setParam(IloCplex.IntParam.SiftDisplay, 0); // ??
			cplex.setParam(IloCplex.DoubleParam.TiLim, limit);
			System.out.println("outputting model");
			cplex.exportModel("/home/vidar/model.lp");

			cplex.solve();

			// Get inactive solution actions
			Set<Operator> inactiveSolActs = getInactiveSolutionActions();

			while (!inactiveSolActs.isEmpty()) {
				for (Operator ch : inactiveSolActs) {
					createCompositionConstraints(ch);
				}

				// Solve new model
				cplex.exportModel("/home/vidar/model2.lp");
				cplex.solve();

				inactiveSolActs = getInactiveSolutionActions();
			}
		} catch (IloException e) {
			e.printStackTrace();
			throw new RuntimeException("Error with IP encoding--see cause", e);
		}
	}

	public Set<Operator> getInactiveSolutionActions() throws EncodingBuildException {
		Set<Operator> inactiveActSet = new LinkedHashSet<Operator>();
		for (Operator a : sas.getOperators()) {
			for (Operator ch : a.getChildren()) {
				try {
					if (cplex.getValue(X[a.getID()][ch.getID()]) > EPSILON) {
						if (ch.isActive() == false) {
							ch.setActive(true);
							inactiveActSet.add(ch);
						}
					}
				} catch (UnknownObjectException e) {
					throw new EncodingBuildException(e);
				} catch (IloException e) {
					throw new EncodingBuildException(e);
				}
			}
		}

		return inactiveActSet;
	}

	public void buildModel() throws EncodingBuildException {
		try {
			solved = false;
			cplex = new IloCplex();

			X = new IloNumVar[sas.getOperators().size()][];
			for (Operator a : sas.getOperators()) {
				X[a.getID()] = new IloNumVar[a.getChildren().size()];
			}
			Z = new IloNumVar[sas.getVariables().size()][];
			for (Variable v : sas.getVariables()) {
				Z[v.getID()] = new IloNumVar[v.getDomain()];
			}
			Gd = new IloNumVar[psp.getGM().numGd];

			consFlow = new IloRange[sas.getVariables().size()][];
			for (Variable v : sas.getVariables()) {
				consFlow[v.getID()] = new IloRange[v.getDomain()];
			}

			dtg = sas.createDTG();
			createActionVariables();
			createGoalVariables();
			// createGoalDepVariables();

			if (!pddl3)
				createObjective();
			else
				createPDDL3Objective();

			createGoalConstraints();
			createFlowConstraints();
			createCompositionConstraints();
			createGoalDep1Constraints();
			createGoalDep2Constraints();
			createConversions();

			// initially we're doing the IP.
			solveRelaxation = false;
			cplex.setParam(IloCplex.IntParam.MIPDisplay, 0);
			cplex.setParam(IloCplex.IntParam.BarDisplay, 0);
			cplex.setParam(IloCplex.IntParam.ConflictDisplay, 0);
			cplex.setParam(IloCplex.IntParam.NetDisplay, 0);
			cplex.setParam(IloCplex.IntParam.SimDisplay, 0); // ??
			cplex.setParam(IloCplex.IntParam.SiftDisplay, 0); // ??
			cplex.setParam(IloCplex.DoubleParam.TiLim, limit);
			System.out.println("outputting model");
			cplex.exportModel("/home/vidar/model.lp");

			cplex.solve();
			System.out.println("OBJ VALUE " + cplex.getObjValue());

		} catch (IloException e) {
			e.printStackTrace();
			throw new RuntimeException("Error with IP encoding--see cause", e);
		}
	}

	private void createPDDL3Objective() throws IloException {
		IloLinearNumExpr obj = cplex.linearNumExpr();
		double sum = 0.0;

		for (Atom atom : sas.getAtoms()) {
			float utility = atom.getUtility();

			if (utility != 0.0f) {
				// add utility for this atom...
				sum += utility;
			}
		}
		// Add utilities of the goal dependencies
		GoalManager gm = psp.getGM();
		for (int i = 0; i < Gd.length; i++) {
			sum += gm.gdUtils[i];
		}

		obj.addTerm(sum, cplex.numVar(1.0, 1.0, IloNumVarType.Int, "ObjFnConstant"));

		for (Atom atom : sas.getAtoms()) {
			float utility = atom.getUtility();

			if (utility != 0.0f) {
				// add utility for this atom...
				obj.addTerm(-utility, Z[atom.getVariable().getID()][atom.getValue()]);
				sum += utility;
			}
		}

		// Add utilities of the goal dependencies
		for (int i = 0; i < Gd.length; i++) {
			obj.addTerm(-gm.gdUtils[i], Gd[i]);
		}

		for (Operator a : sas.getOperators()) {
			for (Operator k : a.getChildren()) {
				obj.addTerm(a.getCost(), X[a.getID()][k.getID()]);
			}
		}

		cplex.addMinimize(obj);
	}

	private void createGoalDep1Constraints() throws IloException {
		// If Gd is selected, then we must select all goals in Gd
		GoalManager gm = psp.getGM();
		for (int i = 0; i < Gd.length; i++) {
			for (int j = 0; j < gm.gdSizes[i]; j++) {
				Atom a = psp.getAtom(gm.gds[i][j]);

				IloLinearNumExpr expr = cplex.linearNumExpr();
				expr.addTerm(1.0, Z[a.getVariable().getID()][a.getValue()]);
				expr.addTerm(-1.0, Gd[i]);

				cplex.addGe(expr, 0.0, "GoalDep1_" + i + "_" + j);
				expr.clear();
			}
		}

	}

	private void createGoalDep2Constraints() throws IloException {
		// If we select all goals in Gd, then we must select Gd
		GoalManager gm = psp.getGM();
		for (int i = 0; i < Gd.length; i++) {
			IloLinearNumExpr expr = cplex.linearNumExpr();

			for (int j = 0; j < gm.gdSizes[i]; j++) {
				Atom a = psp.getAtom(gm.gds[i][j]);

				expr.addTerm(-1.0, Z[a.getVariable().getID()][a.getValue()]);
			}
			expr.addTerm(1.0, Gd[i]);

			cplex.addGe(expr, 1.0 - gm.gdSizes[i], "GoalDep2_" + i);
			expr.clear();
		}
	}

	/**
	 * Must be called at end of the program to free up the cplex license.
	 * 
	 */
	public void end() {
		cplex.end();
	}

	protected IloConversion conversions[];

	public void createConversions() throws IloException {
		int id;

		// To solve the LP relaxation
		conversions = new IloConversion[1 + sas.getOperators().size()
				+ sas.getVariables().size()];
		conversions[0] = cplex.conversion(Gd, IloNumVarType.Float);
		for (int i = 0; i < sas.getOperators().size(); i++) {
			id = sas.getOperators().get(i).getID();
			conversions[i + 1] = cplex.conversion(X[id], IloNumVarType.Float);
		}
		for (int i = 0; i < sas.getVariables().size(); i++) {
			id = sas.getVariables().get(i).getID();
			conversions[i + sas.getOperators().size() + 1] = cplex.conversion(Z[id],
					IloNumVarType.Float);
		}
	}

	public void setRelaxationMode(boolean solveRelaxation) throws IloException {
		// if no change, then nothing to do
		if (solveRelaxation == this.solveRelaxation)
			return;

		this.solveRelaxation = solveRelaxation;
		solved = false;
		// System.out.println("SOLVE RELAX " + solveRelaxation);
		if (solveRelaxation) {
			// To solve the LP relaxation
			for (int i = 0; i < conversions.length; i++) {
				cplex.add(conversions[i]);
			}
			// cplex.add(conversion[0]);
			// cplex.add(cplex.conversion(Y, IloNumVarType.Float));
			// cplex.add(cplex.conversion(YBar, IloNumVarType.Float));
			// cplex.add(cplex.conversion(Gd, IloNumVarType.Float));
			// for (Variable v : sas.getVariables()) {
			// cplex.add(cplex.conversion(Z[v.getID()], IloNumVarType.Float));
			// }

		} else {
			// To solve IP
			for (int i = 0; i < conversions.length; i++) {
				cplex.remove(conversions[i]);
			}
			// cplex.remove(cplex.conversion(X, IloNumVarType.Float));
			// cplex.remove(cplex.conversion(Y, IloNumVarType.Float));
			// cplex.remove(cplex.conversion(YBar, IloNumVarType.Float));
			// cplex.remove(cplex.conversion(Gd, IloNumVarType.Float));
			// for (Variable v : sas.getVariables()) {
			// cplex.remove(cplex.conversion(Z[v.getID()],
			// IloNumVarType.Float));
			// }
			//			
			// cplex.add(cplex.conversion(X, IloNumVarType.Int));
			// cplex.add(cplex.conversion(Y, IloNumVarType.Int));
			// cplex.add(cplex.conversion(YBar, IloNumVarType.Int));
			// cplex.add(cplex.conversion(Gd, IloNumVarType.Int));
			// for (Variable v : sas.getVariables()) {
			// cplex.add(cplex.conversion(Z[v.getID()], IloNumVarType.Int));
			// }
		}
	}

	private String makeArrayString(int[] vars, int[] vals) {
		StringBuilder st = new StringBuilder();
		for (int i = 0; i < vars.length; i++) {
			st.append(vars[i]).append(':').append(vals[i]).append('_');
		}

		return st.toString();
	}

	private void sortArrays(int[] vars, int[] vals) {
		int i, j;
		int tmp;
		int size = vars.length;

		for (i = 0; i < size - 1; i++) {
			for (j = i + 1; j < size; j++) {
				if (vars[i] > vars[j]) {
					tmp = vars[i];
					vars[i] = vars[j];
					vars[j] = tmp;

					tmp = vals[i];
					vals[i] = vals[j];
					vals[j] = tmp;
				}
			}
		}
	}

	/**
	 * Returns set of (child) operators that are applicable (including both
	 * prevails and effects) in the dtg not including node.
	 * 
	 * @param dtg
	 * @param node
	 * @return
	 */
	private Set<Operator> getComplementApplicableActionSet(
			DirectedGraph<ValueVertex, EffectArc> dtg, ValueVertex node) {
		Set<ValueVertex> vertices = dtg.vertexSet();
		Set<Operator> ops = new LinkedHashSet<Operator>();
		for (ValueVertex vv : vertices) {
			if (vv != node)
				ops.addAll(vv.getApplicableActionSet(dtg));
		}

		return ops;
	}

	private Set<Operator> getComplementRelevantActionSet(
			DirectedGraph<ValueVertex, EffectArc> dtg, ValueVertex node) {
		Set<ValueVertex> vertices = dtg.vertexSet();
		Set<Operator> ops = new LinkedHashSet<Operator>();
		for (ValueVertex vv : vertices) {
			if (vv != node)
				ops.addAll(vv.getRelevantActionSet(dtg));
		}

		return ops;
	}

	private Set<Operator> getCompositionOutEdges(int[] vars, int[] vals) {
		Set<Operator> set1, set2, set3;
		// Set<Operator> unionset1 = new LinkedHashSet<Operator>();
		// Set<Operator> unionset2 = new LinkedHashSet<Operator>();
		Set<Operator> intersectset = new LinkedHashSet<Operator>();

		ValueVertex currNode = sas.getVertex(vars[0], vals[0]);
		DirectedGraph<ValueVertex, EffectArc> currDTG = dtg.get(vars[0]);
		// unionset1 = currNode.getApplicableEffectActionSet(currDTG);
		// unionset2 = getComplementApplicableActionSet(currDTG, currNode);
		intersectset = currNode.getApplicableActionSet(currDTG);

		for (int i = 1; i < vars.length; i++) {
			currNode = sas.getVertex(vars[i], vals[i]);
			currDTG = dtg.get(vars[i]);
			// set1 = currNode.getApplicableEffectActionSet(currDTG);
			// set2 = getComplementApplicableActionSet(currDTG, currNode);
			set3 = currNode.getApplicableActionSet(currDTG);

			// unionset1.addAll(set1);
			// unionset2.addAll(set2);
			intersectset.retainAll(set3);
		}

		// unionset1 is now set difference
		// unionset1.removeAll(unionset2);
		// unionset1.addAll(intersectset);

		// DEBUG
		System.out.println("OUT edges :: " + Arrays.toString(vars) + " "
				+ Arrays.toString(vals));
		// System.out.println("SET SIZE " + unionset1.size());

		return intersectset;
	}

	private Set<Operator> getCompositionInEdges(int[] vars, int[] vals) {
		Set<Operator> set1, set2, set3;
		Set<Operator> unionset1 = new LinkedHashSet<Operator>();
		Set<Operator> unionset2 = new LinkedHashSet<Operator>();
		Set<Operator> intersectset = new LinkedHashSet<Operator>();

		ValueVertex currNode = sas.getVertex(vars[0], vals[0]);
		DirectedGraph<ValueVertex, EffectArc> currDTG = dtg.get(vars[0]);
		unionset1 = currNode.getRelevantEffectActionSet(currDTG);
		unionset2 = getComplementRelevantActionSet(currDTG, currNode);
		intersectset = currNode.getRelevantActionSet(currDTG);

		System.out.println("u1::" + unionset1);
		System.out.println("u2::" + unionset2);
		System.out.println("i::" + intersectset);

		for (int i = 1; i < vars.length; i++) {
			currNode = sas.getVertex(vars[i], vals[i]);
			currDTG = dtg.get(vars[i]);
			set1 = currNode.getRelevantEffectActionSet(currDTG);
			set2 = getComplementRelevantActionSet(currDTG, currNode);
			set3 = currNode.getRelevantActionSet(currDTG);

			unionset1.addAll(set1);
			unionset2.addAll(set2);
			intersectset.retainAll(set3);

			System.out.println("s1::" + set1);
			System.out.println("s2::" + set2);
			System.out.println("s3::" + set3);
		}

		System.out.println("fu1::" + unionset1);
		System.out.println("fu2::" + unionset2);
		System.out.println("fi::" + intersectset);

		// unionset1 is now set difference
		unionset1.removeAll(unionset2);
		unionset1.addAll(intersectset);

		System.out.println("ffu1::" + unionset1);
		// DEBUG
		System.out.println("IN edges :: " + Arrays.toString(vars) + " "
				+ Arrays.toString(vals));
		System.out.println("SET SIZE " + unionset1.size());

		return unionset1;
	}

	public void createCompositionConstraints() throws IloException {
		for (Operator a : sas.getOperators()) {
			System.out.println(a.getName());
			for (Operator ch : a.getChildren()) {
				createCompositionConstraints(ch);
			}
		}
	}

	public void createCompositionConstraints(Operator ch) throws IloException {
		int i, j;
		
		if (ch.getPrevailList().size() + ch.getEffectList().size() > 1) {
			System.out.println(ch.getID());
			// Need to create constraint for every prevail-effect pair!
			// Need to create constraint for *all* effects if there are no
			// prevails!
			if (ch.getPrevailList().size() > 0) {
				int[] vars = new int[2];
				int[] vals = new int[2];
				for (i = 0; i < ch.getPrevailList().size(); i++) {
					Prevail p = ch.getPrevailList().get(i);
					vars[i] = p.getVarID();
					vals[i] = p.getValueID();

					for (j = 0; j < ch.getEffectList().size(); j++) {
						Effect e = ch.getEffectList().get(j);
						vars[1] = e.getVariable().getID();
						vals[1] = e.getOldValue();

						createCompositionConstraints(ch, vars, vals);
					}
				}
			} else {
				int[] vars = new int[ch.getEffectList().size()];
				int[] vals = new int[ch.getEffectList().size()];

				for (j = 0; j < ch.getEffectList().size(); j++) {
					Effect e = ch.getEffectList().get(j);
					vars[j] = e.getVariable().getID();
					vals[j] = e.getOldValue();
				}

				createCompositionConstraints(ch, vars, vals);
			}
		}
	}

	private void createCompositionConstraints(Operator ch, int[] vars, int[] vals)
			throws IloException {
		int i;

		sortArrays(vars, vals);
		String condName = makeArrayString(vars, vals);
		if (!compositionMap.containsKey(condName)) {
			compositionMap.put(condName, true);

			// Get in edges
			Set<Operator> actions_in = getCompositionInEdges(vars, vals);
			// System.out.println("IN ACTIONS" + actions_in);
			// Get out edges
			Set<Operator> actions_out = getCompositionOutEdges(vars, vals);
			// System.out.println("OUT ACTIONS" + actions_out);

			// Create constraint
			IloLinearNumExpr expr = cplex.linearNumExpr();

			for (Operator o : actions_in) {
				expr.addTerm(1.0, X[o.getParent().getID()][o.getID()]);
			}
			for (Operator o : actions_out) {
				expr.addTerm(-1.0, X[o.getParent().getID()][o.getID()]);
			}

			int rhs = -1;

			for (i = 0; i < vars.length; i++) {
				if (sas.getInitValue(sas.getVariables().get(vars[i])) != vals[i]) {
					rhs = 0;
					break;
				}
			}
			cplex.addGe(expr, rhs, "CompFlow_" + Arrays.toString(vars) + "_"
					+ Arrays.toString(vals));

			expr.clear();
		}
	}

	/**
	 * Must be called before building the model for the first time. Otherwise
	 * the default will be Float.MAX_VALUE.
	 * 
	 * @param limit
	 *            A floating point number representing the maximum number of
	 *            seconds we wish to spend solving the problem.
	 */
	public void setLimit(float limit) {
		this.limit = limit;
	}

	public float getLimit() {
		return limit;
	}

	public void setSAS(SAS sas) {
		this.sas = sas;
	}

	public void createActionVariables() throws IloException {
		for (Operator a : sas.getOperators()) {
			for (Operator k : a.getChildren()) {
				X[a.getID()][k.getID()] = cplex.numVar(0.0, Double.POSITIVE_INFINITY,
						IloNumVarType.Int, "X_" + a.getID() + "_" + k.getID() + "_"
								+ a.getName());
			}
		}
	}

	/**
	 * Must call this at most once.
	 * 
	 * @throws IloException
	 */
	public void createGoalVariables() throws IloException {
		for (Variable v : sas.getVariables()) {
			int dSize = v.getDomain();
			for (int i = 0; i < dSize; i++) {
				IloNumVar g = cplex.numVar(0.0, 1.0, IloNumVarType.Int, "Z_" + v.getID()
						+ "_" + i);
				Z[v.getID()][i] = g;
				gMap.put(g, new GoalVarVal(v, i, sas.isGoal(v, i)));
			}
		}
	}

	public void createGoalDepVariables() throws IloException {
		// If Gd is selected, then we must select all its goals
		for (int i = 0; i < Gd.length; i++) {
			Gd[i] = cplex.numVar(0.0, 1.0, IloNumVarType.Int, "Gd_" + i);
		}
	}

	public void createObjective() throws IloException {
		System.out.println("START CREATE OBJ");
		IloLinearNumExpr obj = cplex.linearNumExpr();
		for (Operator a : sas.getOperators()) {
			for (Operator k : a.getChildren()) {
				obj.addTerm(-a.getCost(), X[a.getID()][k.getID()]);
				// System.out.println("x: " +
				// X[a.getID()][k.getID()].getName());
			}
		}

		for (Atom atom : sas.getAtoms()) {
			float utility = atom.getUtility();

			if (utility != 0.0f) {
				// add utility for this atom...
				obj.addTerm(utility, Z[atom.getVariable().getID()][atom.getValue()]);
				// System.out.println("z: " +
				// Z[atom.getVariable().getID()][atom.getValue()].getName());
			}
		}

		// Add utilities of the goal dependencies
		GoalManager gm = psp.getGM();
		for (int i = 0; i < Gd.length; i++) {
			obj.addTerm(gm.gdUtils[i], Gd[i]);
			// System.out.println("gd utils: " + gm.gdUtils[i]);
		}

		cplex.addMaximize(obj);
	}

	public void createGoalConstraints() throws IloException {
		HashMap<Variable, ArrayList<Integer>> goalMap = sas.getGoals();
		for (Variable v : sas.getVariables()) {
			// for all variables with goals
			ArrayList<Integer> values = goalMap.get(v);
			if (values != null) {
				for (Integer val : goalMap.get(v)) {
					for (int i = 0; i < v.getDomain(); i++) {
						if (i != val) {
							cplex.addEq(Z[v.getID()][i], 0.0, "Goal_" + v.getID() + "_"
									+ i);
						} else {
							cplex.addEq(Z[v.getID()][i], 1.0, "Goal_" + v.getID() + "_"
									+ i);
						}
					}
				}
			}
		}
	};

	public void createFlowConstraints() throws IloException {
		for (Variable v : sas.getVariables()) {
			int varID = v.getID();
			DirectedGraph<ValueVertex, EffectArc> g = dtg.get(varID);
			for (int valID = 0; valID < v.getDomain(); valID++) {
				IloLinearNumExpr expr = cplex.linearNumExpr();
				for (EffectArc e : g.incomingEdgesOf(sas.getVertex(varID, valID))) {
					// System.out.println(X[e.getEffect().getOperator().getParent().getID()][e.getEffect().getOperator().getID()].getName());
					System.out.println("IN::parent ID: "
							+ e.getEffect().getOperator().getParent().getID());
					// System.out.println("child ID: " +
					// e.getEffect().getOperator().getID() + "\n");
					expr.addTerm(1.0,
							X[e.getEffect().getOperator().getParent().getID()][e
									.getEffect().getOperator().getID()]);
				}
				for (EffectArc e : g.outgoingEdgesOf(sas.getVertex(varID, valID))) {
					System.out.println("OUT::parent ID: "
							+ e.getEffect().getOperator().getParent().getID());
					expr.addTerm(-1.0,
							X[e.getEffect().getOperator().getParent().getID()][e
									.getEffect().getOperator().getID()]);
				}
				expr.addTerm(-1.0, Z[varID][valID]);

				if (valID == sas.getInitValue(v)) {
					consFlow[varID][valID] = cplex.addEq(expr, -1.0, "Flow_" + varID
							+ "_" + valID);
				} else {
					consFlow[varID][valID] = cplex.addEq(expr, 0.0, "Flow_" + varID + "_"
							+ valID);
				}
				expr.clear();
			}
		}
	}

	public void updateInitStateConstraints(List<Integer> newVars) throws IloException {
		solved = false;
		updateFlowConstraints(newVars);
		// set the new initial values
		for (int i = 0; i < newVars.size(); i++) {
			sas.setNewInitValue(sas.getVariables().get(i), newVars.get(i));
		}
		// cplex.exportModel("/home/vidar/model2.lp");
	}

	protected void updateFlowConstraints(List<Integer> newVars) throws IloException {
		for (Variable v : sas.getVariables()) {
			// update old
			consFlow[v.getID()][sas.getInitValue(v)].setBounds(0.0, 0.0);

			// update new
			consFlow[v.getID()][newVars.get(v.getID())].setBounds(-1.0, -1.0);
		}
	}

	protected boolean closeToOne(double val) {
		if (val >= 0.99 && val <= 1.01) {
			return true;
		}

		return false;
	}

	protected boolean closeToZero(double val) {
		if (val >= -0.01 && val <= 0.01) {
			return true;
		}

		return false;
	}

	/**
	 * Call before performing any operations that involve STRIPS (i.e., getting
	 * new goals or getting the relaxed plan).
	 * 
	 * @param psp
	 */
	public void setInterface(PSPInterface psp) {
		this.psp = psp;
	}

	public float solve(StateInfo si) throws EncodingBuildException {
		try {
			List<Integer> vals = psp.getNewVariableValues(si);
			updateInitStateConstraints(vals);
			if (cplex.solve() && cplex.getStatus() == IloCplex.Status.Optimal) {
				// System.out.println("OBJ VALUE " + cplex.getObjValue());
				return (float) cplex.getObjValue();
			} else {
				throw new EncodingBuildException("Problem solving encoding: "
						+ cplex.getStatus());
			}
		} catch (IloException e) {
			throw new EncodingBuildException(e);
		}
	}

	/**
	 * This is for the optimal search to get the new set of goals for the RMTPG
	 * to solve...
	 * 
	 * @return
	 * @throws EncodingBuildException
	 *             When anything in cplex goes wrong this will be thrown.
	 */
	public ArrayList<Integer> getNewGoals() throws EncodingBuildException {
		ArrayList<Atom> atoms = new ArrayList<Atom>();
		double[] maxVarValues = new double[sas.getVariables().size()];
		int[] maxValIdx = new int[maxVarValues.length];
		for (int i = 0; i < maxValIdx.length; i++) {
			maxValIdx[i] = -1;
		}
		try {
			// goals
			for (Entry<IloNumVar, GoalVarVal> g : gMap.entrySet()) {
				double value = cplex.getValue(g.getKey());
				GoalVarVal gvv = g.getValue();
				// if this is the maximum value and this is a goal value, then
				// indicate that this is a goal to be achieved...
				if (value > maxVarValues[gvv.var.getID()] && gvv.isGoal) {
					maxVarValues[gvv.var.getID()] = value;
					maxValIdx[gvv.var.getID()] = gvv.val;
				}
			}
			// end();
		} catch (UnknownObjectException e) {
			e.printStackTrace();
			throw new EncodingBuildException(e);
		} catch (IloException e) {
			e.printStackTrace();
			throw new EncodingBuildException(e);
		}
		for (int i = 0; i < maxVarValues.length; i++) {
			if (maxValIdx[i] >= 0)
				atoms.add(sas.getVariables().get(i).getAtom(maxValIdx[i]));
		}

		return psp.convertGoals(atoms);
	}

	/**
	 * Call to get a set of new goals, returned using the STRIPS predIDs. This
	 * is for the up-front goal selection.
	 */
	public ArrayList<Integer> newGoals() throws EncodingBuildException {
		ArrayList<Atom> atoms = new ArrayList<Atom>();
		try {
			if (cplex.solve()) {
				// goals
				for (Entry<IloNumVar, GoalVarVal> g : gMap.entrySet()) {
					if (closeToOne(cplex.getValue(g.getKey()))) {
						GoalVarVal gvv = g.getValue();
						atoms.add(gvv.var.getAtom(gvv.val));
					}
				}
			}
			end();
		} catch (UnknownObjectException e) {
			e.printStackTrace();
			throw new EncodingBuildException(e);
		} catch (IloException e) {
			e.printStackTrace();
			throw new EncodingBuildException(e);
		}

		return psp.convertGoals(atoms);
	}

	class GoalVarVal {
		public Variable var;

		public int val;

		boolean isGoal;

		public GoalVarVal(Variable v, int va, boolean isGoal) {
			this.var = v;
			this.val = va;
			this.isGoal = isGoal;
		}
	}

	public float solve() throws EncodingBuildException {
		try {
			if (!cplex.solve()) {
				throw new EncodingBuildException("CPLEX solve failed when solving for "
						+ (solveRelaxation ? "LP" : "IP"));
			}
			solved = true;
			// System.out.println("OBJ VALUE " + cplex.getObjValue());
			hValue = cplex.getObjValue();

			// for (int i = 0;i<Z.length;i++) {
			// for (int j=0;j<Z[i].length;j++) {
			// System.out.println(Z[i][j].getName() + ": " +
			// cplex.getValue(Z[i][j]));
			// }
			// }
			//
			// for (int i = 0;i<X.length;i++) {
			// if (X[i].getName().startsWith("X_nav"))
			// System.out.println(X[i].getName() + ": " + cplex.getValue(X[i]));
			// }

			return (float) hValue;
		} catch (IloException e) {
			throw new EncodingBuildException(e);
		}
	}

	public boolean noActions() throws EncodingBuildException {
		// double utilSum = 0.0;
		// try {
		// GoalManager gm = psp.getGM();
		//			
		// for (int i=0;i<Gd.length;i++) {
		// utilSum += cplex.getValue(Gd[i]) * gm.gdUtils[i];
		// }
		//			
		// psp.convertPreds(Arrays.asList(gm.gID));
		// } catch (UnknownObjectException e) {
		// throw new EncodingBuildException(e);
		// } catch (IloException e) {
		// throw new EncodingBuildException(e);
		// }

		boolean noActs = true;

		// we just want to see all zeros in the actions
		for (int i = 0; i < X.length; i++) {
			for (int j = 0; j < X[i].length; j++) {
				try {
					if (!closeToZero(cplex.getValue(X[i][j]))) {
						// System.out.println(sas.getOperators().get(i).getName()
						// +
						// " " + cplex.getValue(X[i][j]));
						noActs = false;
					}
				} catch (UnknownObjectException e) {
					throw new EncodingBuildException(e);
				} catch (IloException e) {
					throw new EncodingBuildException(e);
				}
			}
		}
		return noActs;
	}

	public void printPositiveActions() throws EncodingBuildException {
		// we just want to see all zeros in the actions
		for (int i = 0; i < X.length; i++) {
			for (int j = 0; j < X[i].length; j++) {
				try {
					if (cplex.getValue(X[i][j]) > EPSILON) {
						System.out.println(" AAA " + sas.getOperators().get(i).getName()
								+ " " + cplex.getValue(X[i][j]));
					}
				} catch (UnknownObjectException e) {
					throw new EncodingBuildException(e);
				} catch (IloException e) {
					throw new EncodingBuildException(e);
				}
			}
		}
	}

	public ArrayList<GAction> getActions() throws EncodingBuildException {
		ArrayList<GAction> planActions;
		if (!solved)
			throw new EncodingBuildException("You must solve the encoding first.");
		if (!solveRelaxation) {
			planActions = new ArrayList<GAction>();
			for (int i = 0; i < X.length; i++) {
				for (int j = 0; j < X[i].length; j++) {
					IloNumVar a = X[i][j];
					try {
						if (closeToOne(cplex.getValue(a))) {
							planActions.add(psp.convertOperator(i));
						}
					} catch (UnknownObjectException e) {
						throw new EncodingBuildException(e);
					} catch (IloException e) {
						throw new EncodingBuildException(e);
					}
				}
			}
		} else {
			throw new EncodingBuildException(
					"Actions unavailable when only the relaxation has been solved.");
		}
		if (planActions.isEmpty())
			noActions = true;
		else
			noActions = false;
		return planActions;
	}

	public ArrayList<GAction> getLPActions() throws EncodingBuildException {
		ArrayList<GAction> planActions;
		if (!solved)
			throw new EncodingBuildException("You must solve the encoding first.");
		planActions = new ArrayList<GAction>();
		for (int i = 0; i < X.length; i++) {
			for (int j = 0; j < X[i].length; j++) {
				IloNumVar a = X[i][j];
				try {
					double value = cplex.getValue(a);
					if (closeToOne(cplex.getValue(a))) {
						planActions.add(psp.convertOperator(i));
					} else if (value > EPSILON) {
						// if the value is not one but greater than epsilon...
						planActions.clear(); // then non-integer!
						break;
					}
				} catch (UnknownObjectException e) {
					throw new EncodingBuildException(e);
				} catch (IloException e) {
					throw new EncodingBuildException(e);
				}
			}
		}
		// if (planActions.isEmpty()) noActions = true; else noActions = false;
		return planActions;
	}

	public int[] roundedActionsInt(Random randGen) throws EncodingBuildException {
		LinkedList<Integer> planActions;
		if (!solved)
			throw new EncodingBuildException("You must solve the encoding first.");
		planActions = new LinkedList<Integer>();
		for (int i = 0; i < X.length; i++) {
			for (int j = 0; j < X[i].length; j++) {
				IloNumVar a = X[i][j];
				try {
					double value = cplex.getValue(a);
					if (value > EPSILON) {
						if (value < randGen.nextDouble()) {
							planActions.add(psp.convertOperator(i).getID()); // then
							// non-integer!
							// System.out.println(value + " " +
							// Grounding.getActSig(psp.convertOperator(i).getID()));
						}
					}
				} catch (UnknownObjectException e) {
					throw new EncodingBuildException(e);
				} catch (IloException e) {
					throw new EncodingBuildException(e);
				}
			}
		}

		int[] rp = new int[planActions.size()];
		int idx = 0;
		for (Integer i : planActions) {
			rp[idx] = i;
			idx++;
		}
		// System.out.println("XXXXXXXXXXXXXXXXXXXX");
		// if (planActions.isEmpty()) noActions = true; else noActions = false;
		return rp;
	}

	/**
	 * Randomized rounding for the actions...
	 * 
	 * @return
	 * @throws EncodingBuildException
	 */
	public ArrayList<GAction> roundedActions() // Random randGen)
			throws EncodingBuildException {
		ArrayList<GAction> planActions;
		if (!solved)
			throw new EncodingBuildException("You must solve the encoding first.");
		planActions = new ArrayList<GAction>();
		for (int i = 0; i < X.length; i++) {
			for (int j = 0; j < X[i].length; j++) {
				IloNumVar a = X[i][j];
				try {
					double value = cplex.getValue(a);
					if (value > EPSILON) {
						// if (value > randGen.nextDouble()) {
						// System.out.println("BBB " +
						// sas.getOperators().get(i).getName());
						planActions.add(psp.convertOperator(i)); // then
						// non-integer!
						// System.out.println(value + " " +
						// Grounding.getActSig(psp.convertOperator(i).getID()));
						// }
					}
				} catch (UnknownObjectException e) {
					throw new EncodingBuildException(e);
				} catch (IloException e) {
					throw new EncodingBuildException(e);
				}
			}
		}
		// System.out.println("XXXXXXXXXXXXXXXXXXXX");
		// if (planActions.isEmpty()) noActions = true; else noActions = false;
		return planActions;
	}

	public boolean hasAction(int aid) throws EncodingBuildException {
		int id = -1;
		if (!solved)
			throw new EncodingBuildException("You must solve the encoding first.");
		for (int i = 0; i < X.length; i++) {
			for (int j = 0; j < X[i].length; j++) {
				IloNumVar a = X[i][j];
				try {
					double value = cplex.getValue(a);
					id = psp.convertOperator(i).getID();
					if (id == aid && value > EPSILON)
						return true;
				} catch (UnknownObjectException e) {
					throw new EncodingBuildException(e);
				} catch (IloException e) {
					throw new EncodingBuildException(e);
				}
			}
		}
		return false;
	}

}
